#!/usr/bin/env python
# -*- coding: utf-8 -*-

import rospy
from geometry_msgs.msg import PoseStamped
#-1-right
#0-up
#1-left
#big-down

def go_to(num):
    info=[
        [0.3][0.3]['U'][0],       #0起点
        [1.2][0.3]['U'][0],       #1红灯1
        [3.7][0.3]['L'][0],       #2转向1
        [3.7][1.5]['L'][0],       #3转向2
        [2.9][1.5]['R'][0],       #4社区A
        [2.9][1.5]['L'][0],       #5社区B
        [2.1][1.5]['L'][0],       #6转向3
        [2.1][2.1]['D'][0],       #7楼1
        [2.1][2.1]['L'][0],       #8红灯2
        [2.1][3.0]['U'][0],       #9垃圾桶
        [2.1][3.0]['D'][0],       #10楼3.0
        [2.1][3.7]['D'][0],       #11转向4
        [0.9][3.7]['D'][0],       #12车1
        [0.9][3.1]['D'][0],       #13车2
        [0.9][2.5]['D'][0],       #14车3
        [0.9][2.5]['U'][0],       #15楼2
        [0.9][2.0]['D'][0],       #16电动车1
        [0.9][0.8]['D'][0],       #17电动车2
        [0.9][0.3]['D'][0],       #18入库点
        [0.3][0.3]['D'][0],       #19入库
        [0.3][0.3]['U'][0],       #20转向
        ]
    move_to_fixed_poin(info[num][0]info[num][1],info[num][2])
    #rospy.sleep(info[num][3])

    

def move_to_fixed_poin(x,y,f):
    # 初始化ROS节点
    rospy.init_node('move_robot_to_fixed_point')
    # 创建一个Publisher，发布到'move_base_simple/goal'话题
    pub = rospy.Publisher('/move_base_simple/goal', PoseStamped, queue_size=10)
    # 创建一个PoseStamped消息作为目标位置
    goal = PoseStamped()
    goal.header.frame_id = "map"  # 设置坐标系为"map"
    goal.pose.position.x = x  # 设置x坐标
    goal.pose.position.y = y  # 设置y坐标
    goal.pose.orientation.w = 1.0  # 设置四元数的w部分
    if f=='L':
        a=1
    if f=='R':
        a=-1
    if f=='U':
        a=0
    if f=='D':
        a=100
    goal.pose.orientation.z = a  # 设置四元数的z部分
    # 等待1秒钟
    #rospy.sleep(1)
    # 发布目标位置
    pub.publish(goal)


if __name__ == '__main__':
    #try:
	go_to(0)
    rospy.sleep(15)
    go_to(1)
    rospy.sleep(15)
    go_to(1)
    go_to(2)
    go_to(3)
    go_to(4)
    go_to(5)
    go_to(6)
    go_to(7)
    go_to(8)
    go_to(9)
    go_to(10)
    go_to(11)
    go_to(12)
    go_to(13)
    go_to(14)
    go_to(15)
    go_to(16)
    go_to(17)
    go_to(18)
    go_to(19)
    go_to(20)









    #except rospy.ROSInterruptException:
    #    pass


















